/**
 * @file Hill48Plasticity.cpp
 * @brief Implementation of Hill48 anisotropic plasticity model
 * 
 * This implements the Hill48 yield criterion for anisotropic plasticity,
 * including iterative stress update and plastic strain calculation.
 */

#include <vector>
#include <cmath>
#include <iostream>
#include <string>
#include <algorithm>

// Define precision type
#ifdef DOUBLE_PRECISION
using Real = double;
#else
using Real = float;
#endif

// Constants
constexpr Real ZERO = 0.0;
constexpr Real HALF = 0.5;
constexpr Real THREE = 3.0;
constexpr Real TWO = 2.0;
constexpr int MAX_ITERATIONS = 50;
constexpr Real CONVERGENCE_TOL = 0.001;

/**
 * @brief Implements Hill48 anisotropic plasticity model
 * 
 * @param stress Stress tensor components (input/output)
 * @param hf Hill48 F parameter
 * @param hn Hill48 N parameter
 * @param hh Hill48 H parameter
 * @param numElements Number of elements to process
 * @param yield Yield stress values
 * @param plasticStrain1 Plastic strain part 1
 * @param plasticStrain2 Plastic strain part 2
 * @param materialParams Material parameters array
 * @param poisson Poisson's ratio
 * @param plasticStrainXX Output plastic strain XX component
 * @param plasticStrainYY Output plastic strain YY component
 * @param plasticStrainXY Output plastic strain XY component
 * @param effectivePlasticStrain Effective plastic strain
 * @param thicknessStrain Thickness strain component
 * @param phaseFlag Phase flag (special handling if =1)
 * @param elementIndices Element indices array
 * @param strainIncrement Strain increment
 * @param fsh Shear factor
 * @param elementFlags Element flags array
 * @param numPlasticElements Output count of plastic elements
 * @param messageBuffer Output message buffer for warnings
 */
void Hill48Plasticity(
    std::vector<std::array<Real, 5>>& stress,
    const std::vector<Real>& hf,
    const std::vector<Real>& hn,
    const std::vector<Real>& hh,
    int numElements,
    const std::vector<Real>& yield,
    std::vector<Real>& plasticStrain1,
    std::vector<Real>& plasticStrain2,
    const std::vector<std::array<Real, 8>>& materialParams,
    const std::vector<Real>& poisson,
    std::vector<Real>& plasticStrainXX,
    std::vector<Real>& plasticStrainYY,
    std::vector<Real>& plasticStrainXY,
    std::vector<Real>& effectivePlasticStrain,
    std::vector<Real>& thicknessStrain,
    int phaseFlag,
    const std::vector<int>& elementIndices,
    std::vector<Real>& strainIncrement,
    const std::vector<Real>& fsh,
    const std::vector<int>& elementFlags,
    int& numPlasticElements,
    std::string& messageBuffer)
{
    // Temporary storage
    std::vector<Real> equivalentStress(numElements);
    std::vector<Real> strainIncrementInitial(numElements);
    std::vector<Real> sig1(numElements), sig2(numElements), sig3(numElements);
    std::vector<Real> sig4(numElements), sig5(numElements);
    std::vector<Real> cons1(numElements), cons2(numElements), cons3(numElements);
    std::vector<Real> cons4(numElements), cons5(numElements), cons6(numElements);
    std::vector<Real> conv(numElements);
    std::vector<int> plasticElementIndices(numElements);
    std::vector<int> activeElementIndices(numElements);

    // Calculate equivalent stress using Hill48 criterion
    for (int i = 0; i < numElements; ++i) {
        equivalentStress[i] = stress[i][0] * stress[i][0] + 
                            HALF * hf[i] * stress[i][1] * stress[i][1] +
                            hn[i] * stress[i][2] * stress[i][2] -
                            hh[i] * stress[i][0] * stress[i][1] +
                            THREE * stress[i][3] * stress[i][3] + 
                            THREE * stress[i][4] * stress[i][4];
        equivalentStress[i] = std::sqrt(std::max(ZERO, equivalentStress[i]));
    }

    numPlasticElements = 0;

    // Identify elements undergoing plastic deformation
    if (phaseFlag != 1) {
        for (int i = 0; i < numElements; ++i) {
            if (equivalentStress[i] > yield[i]) {
                plasticElementIndices[numPlasticElements] = i;
                activeElementIndices[numPlasticElements] = i;
                numPlasticElements++;
            }
        }
    } else {
        for (int i = 0; i < numElements; ++i) {
            if (equivalentStress[i] > yield[i]) {
                plasticElementIndices[numPlasticElements] = i;
                activeElementIndices[numPlasticElements] = i;
                plasticStrain2[i] = plasticStrain1[i] + plasticStrain2[i];
                numPlasticElements++;
            } else {
                plasticStrain2[i] = materialParams[i][0] + plasticStrain2[i];
            }
        }
    }

    int remainingPlasticElements = numPlasticElements;

    // Precompute constants for plastic elements
    for (int j = 0; j < numPlasticElements; ++j) {
        int i = plasticElementIndices[j];
        Real g = materialParams[i][5];
        Real a1 = materialParams[i][6];
        Real a2 = materialParams[i][7];
        Real yInv = 1.0 / yield[i];

        cons1[i] = yInv * (a1 - HALF * hh[i] * a2);
        cons2[i] = yInv * (a2 - HALF * hh[i] * a1);
        cons3[i] = yInv * HALF * (hf[i] * a2 - hh[i] * a1);
        cons4[i] = yInv * HALF * (hf[i] * a1 - hh[i] * a2);
        cons5[i] = yInv * g * hn[i];
        cons6[i] = THREE * yInv * g * fsh[i];

        // Compute DK factor
        Real hh2 = hh[i] * hh[i];
        Real dk1 = (4.0 + hh2 - 4.0 * hh[i] * poisson[i]) * stress[i][0] * stress[i][0];
        dk1 += (hf[i] * hf[i] + hh2 - 2.0 * hh[i] * poisson[i] * hf[i]) * stress[i][1] * stress[i][1];
        dk1 += 2.0 * (poisson[i] * (hh2 + 2.0 * hf[i]) - hh[i] * (hf[i] + 2.0)) * 
               stress[i][0] * stress[i][1];
        dk1 += 2.0 * (1.0 - poisson[i]) * hn[i] * hn[i] * stress[i][2] * stress[i][2];
        dk1 += 18.0 * (1.0 - poisson[i]) * fsh[i] * 
               (stress[i][3] * stress[i][3] + stress[i][4] * stress[i][4]);

        Real dk = ((1.0 - poisson[i]) * equivalentStress[i] * (equivalentStress[i] - yield[i])) / 
                 (materialParams[i][5] * dk1);

        strainIncrement[i] = TWO * dk * yield[i];
        strainIncrementInitial[i] = strainIncrement[i];
    }

    // Iterative stress update
    for (int iter = 0; iter < MAX_ITERATIONS; ++iter) {
        if (remainingPlasticElements == 0) break;

        // Update stresses for active elements
        for (int j = 0; j < remainingPlasticElements; ++j) {
            int i = activeElementIndices[j];
            
            Real fac1 = strainIncrement[i] * cons1[i] + 1.0;
            Real fac2 = strainIncrement[i] * cons2[i];
            Real fac3 = strainIncrement[i] * cons3[i];
            Real fac4 = strainIncrement[i] * cons4[i] + 1.0;
            Real fac = 1.0 / (fac1 * fac4 - fac2 * fac3);
            
            Real fac5 = fac4 * fac;
            Real fac6 = -fac2 * fac;
            Real fac7 = -fac3 * fac;
            Real fac8 = fac1 * fac;
            Real fac9 = 1.0 / (strainIncrement[i] * cons6[i] + 1.0);
            
            sig1[i] = fac5 * stress[i][0] + fac7 * stress[i][1];
            sig2[i] = fac6 * stress[i][0] + fac8 * stress[i][1];
            sig3[i] = stress[i][2] / (strainIncrement[i] * cons5[i] + 1.0);
            sig4[i] = stress[i][3] * fac9;
            sig5[i] = stress[i][4] * fac9;
            
            // Compute new equivalent stress
            equivalentStress[i] = sig1[i] * sig1[i] + 
                                 HALF * hf[i] * sig2[i] * sig2[i] +
                                 hn[i] * sig3[i] * sig3[i] -
                                 hh[i] * sig1[i] * sig2[i] +
                                 THREE * sig4[i] * sig4[i] + 
                                 THREE * sig5[i] * sig5[i];
            equivalentStress[i] = std::sqrt(equivalentStress[i]);
            
            conv[i] = std::abs(equivalentStress[i] - yield[i]) / yield[i];
        }

        // Check convergence and update active elements
        remainingPlasticElements = 0;
        for (int j = 0; j < numPlasticElements; ++j) {
            int i = plasticElementIndices[j];
            
            if (strainIncrement[i] < 0.0) {
                // Error case - would need proper error handling
                int errorCode = elementFlags[i * 6 + 5]; // Assuming IL is 6 values per element
            } 
            else if (conv[i] > CONVERGENCE_TOL) {
                activeElementIndices[remainingPlasticElements] = i;
                strainIncrementInitial[i] = (yield[i] - equivalentStress[i]) * 
                                          strainIncrementInitial[i] / 
                                          (equivalentStress[i] - equivalentStress[i]);
                strainIncrement[i] += strainIncrementInitial[i];
                remainingPlasticElements++;
            }
        }
    }

    // Warning if not all elements converged
    if (remainingPlasticElements > 0) {
        messageBuffer = "<<== *** WARNING *** NO CONVERGENCE ACHIEVED AFTER " + 
                       std::to_string(MAX_ITERATIONS) + " ITERATIONS FOR " +
                       std::to_string(remainingPlasticElements) + " ELEMENTS";
        std::cout << messageBuffer << std::endl;
    }

    // Final update of stresses and plastic strains
    for (int j = 0; j < numPlasticElements; ++j) {
        int i = plasticElementIndices[j];
        
        stress[i][0] = sig1[i];
        stress[i][1] = sig2[i];
        stress[i][2] = sig3[i];
        stress[i][3] = sig4[i];
        stress[i][4] = sig5[i];
        
        Real a3i = HALF * strainIncrement[i] / yield[i];
        plasticStrainXX[i] = a3i * (TWO * sig1[i] - hh[i] * sig2[i]);
        plasticStrainYY[i] = a3i * (hf[i] * sig2[i] - hh[i] * sig1[i]);
        plasticStrainXY[i] = a3i * (hn[i] * sig3[i]);
        
        effectivePlasticStrain[i] += strainIncrement[i];
        thicknessStrain[i] -= a3i * (1.0 + materialParams[i][2]) * 
                             ((TWO - hh[i]) * sig1[i] + (hf[i] - hh[i]) * sig2[i]);
    }
}

/**
 * @brief Example usage of the Hill48 plasticity model
 */
void ExampleUsage() {
    const int numElements = 100;
    std::vector<std::array<Real, 5>> stress(numElements);
    std::vector<Real> hf(numElements, 1.0), hn(numElements, 1.0), hh(numElements, 1.0);
    std::vector<Real> yield(numElements, 250.0);
    std::vector<Real> plasticStrain1(numElements), plasticStrain2(numElements);
    std::vector<std::array<Real, 8>> materialParams(numElements);
    std::vector<Real> poisson(numElements, 0.3);
    std::vector<Real> plasticStrainXX(numElements), plasticStrainYY(numElements);
    std::vector<Real> plasticStrainXY(numElements);
    std::vector<Real> effectivePlasticStrain(numElements);
    std::vector<Real> thicknessStrain(numElements);
    std::vector<int> elementIndices(numElements);
    std::vector<Real> strainIncrement(numElements);
    std::vector<Real> fsh(numElements, 1.0);
    std::vector<int> elementFlags(numElements * 6); // Assuming 6 flags per element
    int numPlasticElements = 0;
    std::string messageBuffer;
    int phaseFlag = 0;

    // Initialize some example data
    for (int i = 0; i < numElements; ++i) {
        stress[i] = {300.0, 200.0, 100.0, 50.0, 50.0}; // Over yield for most
        materialParams[i] = {0.0, 0.0, 0.3, 0.0, 0.0, 80000.0, 1.0, 1.0};
        elementIndices[i] = i;
    }

    // Call the plasticity model
    Hill48Plasticity(stress, hf, hn, hh, numElements, yield, plasticStrain1,
                    plasticStrain2, materialParams, poisson, plasticStrainXX,
                    plasticStrainYY, plasticStrainXY, effectivePlasticStrain,
                    thicknessStrain, phaseFlag, elementIndices, strainIncrement,
                    fsh, elementFlags, numPlasticElements, messageBuffer);

    // Output results
    std::cout << "Number of plastic elements: " << numPlasticElements << std::endl;
    std::cout << "First element stresses after update: ";
    for (const auto& s : stress[0]) {
        std::cout << s << " ";
    }
    std::cout << std::endl;
}